/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.templates.commands.DriveWithJoystick;
import edu.wpi.first.wpilibj.Gyro;
//import edu.wpi.first.wpilibj.Accelerometer;
import edu.wpi.first.wpilibj.ADXL345_I2C;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 *
 * @author Wendy Chan
 */
public class DriveSystem extends Subsystem {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
    private RobotDrive drive;
    private Gyro gyro;
    private ADXL345_I2C accel;

    double gyro_o, gyro_n, gyro_filt; //previous gyro readout, new gyro readout, filtered gyro;
    double acc_x, acc_y, acc_z; //accelerometer readouts (x, y, z axes)
    double accel_o, accel_n, accel_filt;
    double tilt, tilt_filt; //a.k.a. angle of inclination on bridge
    double ky; //scaling factor for calculating tilt
    double zo; //twist deadband

    //private static DriveSystem instance = null;

    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
        setDefaultCommand(new DriveWithJoystick());
    }

    public DriveSystem(){
        gyro = new Gyro(RobotMap.gyrochan);
        accel = new ADXL345_I2C (1, ADXL345_I2C.DataFormat_Range.k4G); //can use k2, k4, k8, or k16 -- ex. k8G sets range as +/- 8G
        drive = new RobotDrive(RobotMap.frontLeftMotor, RobotMap.rearLeftMotor, RobotMap.frontRightMotor, RobotMap.rearRightMotor);
        drive.setSafetyEnabled(false);
        ky = 90.0/56.25; //accelerometer scaling factor:  at 90 deg (actual), accelerometer read about 57 degrees (raw)
    }
    /*
    public static DriveSystem getInstance() {
        if (instance == null)
            instance = new DriveSystem();
        return instance;
    }
     *
     */

    public void driveWithJoy(double y, double twist){
        //code to deadband sensitivity of joystick twist.
        //less than zo is changed to 0:
        if (twist < zo && twist > -zo){
            twist = 0;
        }
        //zo to 1 is scaled to 0 to 1 here:
        else if(twist >= zo){
            twist = (twist - zo)/(1 - zo);
        }
        //-1 to -zo is scaled to -1 to 0 here:
        else if(twist <= -zo){
            twist = (twist + zo)/(1 - zo);
        }

        //drive.mecanumDrive_Cartesian(x, y, twist, gyro);
        drive.arcadeDrive(-y, -twist);
        System.out.println("Drive Me");
    }

    public void initsensors(){
        gyro.reset();
        gyro_o = gyro.getAngle();
        accel_o = accel.getAcceleration(ADXL345_I2C.Axes.kY)* ky * 180.0 / 3.1416;
    }

    public void readgyro(){
        gyro_n = gyro.getAngle();
        //System.out.println("Gyro Angle (Raw) = " + gyro_n);
        gyro_filt = (gyro_o - gyro_n) / 2; //high pass filter
        gyro_o = gyro_n;
    }

    public void readaccel(){
        acc_x = accel.getAcceleration(ADXL345_I2C.Axes.kX);
        //System.out.println("Acceleration (X axis)= " + accread);
        acc_y = accel.getAcceleration(ADXL345_I2C.Axes.kY);
        //System.out.println("Acceleration (Y axis)= " + accread);
        acc_z = accel.getAcceleration(ADXL345_I2C.Axes.kZ);
        //System.out.println("Acceleration (Z axis)= " + accread);
        accel_n = acc_y * ky * 180.0 / 3.1416; //convert to degrees
        accel_filt = (accel_o + accel_n) / 2; //low pass filter
        accel_o = accel_n;
    }

    public void balance(){
        //calculate tilt a.k.a. angle of inclination
        //tilt = acc_y * ky * 180.0 / 3.1416;
        tilt_filt = gyro_filt + accel_filt; //complimentary filter
        //System.out.println("Tilt Angle = " + tilt + " degrees");

        //Test this balancing drive algorithm after verifying and adjusting accelerometer readings
        /*
        if (tilt > 10){
            drive.arcadeDrive(0, 1.0);
        }
        if (tilt <= 10 && tilt > 1.0){
            drive.arcadeDrive(0, 0.5);
        }
        if (tilt < -1.0 && tilt >= -10.0){
            drive.arcadeDrive(0, -0.5);
        }
        if (tilt < -10.0){
            drive.arcadeDrive(0, -1.0);
        }
        */
    }

    public void readsensors(){
        SmartDashboard.putDouble("Gyro Angle (Raw/Drifting)", gyro_n);
        SmartDashboard.putDouble("Gyro Angle (Filtered)", gyro_filt);
        SmartDashboard.putDouble("Accelerometer Y in degrees (Raw)", accel_n);
        SmartDashboard.putDouble("Accelerometer Y in degrees (Filtered)", accel_filt);
        SmartDashboard.putDouble("Tilt Angle Y in degrees (Filtered)", tilt_filt);
    }

}